#include "MiniCloud.h"

#include <QDebug>
#include <ctime>

using namespace XYF;

#define ITERATION 100

MiniCloud::MiniCloud(const MiniArray<Vector3f> &points) : m_points(points)
{
}

int MiniCloud::GetType()
{
    return MiniObject::cloud;
}

int MiniCloud::GetPointsNum() const
{
    return m_points.size();
}

const MiniArray<Vector3f> &MiniCloud::GetPoints() const
{
    return m_points;
}

MiniArray<Vector3f> &MiniCloud::GetPoints()
{
    return m_points;
}

MiniBSplineSurface *MiniCloud::EnergyFittingSurface(MiniArray<Real> &u, MiniArray<Real> &v, int Nu, int Nv, int p, int q, MiniArray<Real> &w, Real alpha, Real beta) const
{
    // N 个控制点
    int nu = Nu - 1;
    int nv = Nv - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineSurface *spline = new MiniBSplineSurface(nu + 1, nv + 1, p, q);
    MiniArray2D<Vector3f> &cpts = spline->GetPoints();

    // 填充最小二乘系数矩阵
    Eigen::SparseMatrix<Real> A(m + 1, (nu + 1) * (nv + 1));
    Eigen::SparseMatrix<Real> AT((nu + 1) * (nv + 1), m + 1);
    for (int k = 0; k < m + 1; k++)
    {
        int uSpan = FindSpan(u[k], spline->GetUDegree(), spline->GetUKnots());
        int vSpan = FindSpan(v[k], spline->GetVDegree(), spline->GetVKnots());

        MiniArray<Real> Bu = BasisFuncs(uSpan, u[k], spline->GetUDegree(), spline->GetUKnots());
        MiniArray<Real> Bv = BasisFuncs(vSpan, v[k], spline->GetVDegree(), spline->GetVKnots());

        // 每次填充 (p + 1) * (q + 1) 规模的元素
        int uIndex = (uSpan - p) * (nv + 1);
        for (int i = 0; i < p + 1; i++)
        {
            int vIndex = vSpan - q;
            for (int j = 0; j < q + 1; j++)
            {
                // 同时设置 A 与 A^T，注意 AT 要乘上权重 w
                A.insert(k, uIndex + vIndex + j) = Bu[i] * Bv[j];
                AT.insert(uIndex + vIndex + j, k) = Bu[i] * Bv[j] * w[k];
            }
            uIndex += nv + 1;
        }
    }

    // 构造含有能量项约束的系数矩阵，先计算出 Gauss 积分权和节点
    GaussIntegrator I(3);
    Vector nodes = I.nodes();
    Vector weights = I.weights();

    // 分别沿着 u,v 方向计算积分，然后保存
    Matrix Bu0(nu + 1, nu + 1);
    Matrix Bu1(nu + 1, nu + 1);
    Matrix Bu2(nu + 1, nu + 1);
    Matrix Bv0(nv + 1, nv + 1);
    Matrix Bv1(nv + 1, nv + 1);
    Matrix Bv2(nv + 1, nv + 1);

    MiniArray<Real> &uknots = spline->GetUKnots();
    for (int i = p; i < nu + 1; i++)
    {
        Real mid = (uknots[i + 1] + uknots[i]) / 2;
        Real ratio = (uknots[i + 1] - uknots[i]) / 2;

        // 三个积分节点分别计算值相加
        for (int j = 0; j < 3; j++)
        {
            MiniArray2D<Real> Bu = DerivBasisFuncs(2, i, mid + nodes[j] * ratio, p, uknots);
            for (int ui = 0; ui < p + 1; ui++)
            {
                for (int uj = 0; uj < p + 1; uj++)
                {
                    Bu0(i - p + ui, i - p + uj) += Bu[0][ui] * Bu[0][uj] * weights[j];
                    Bu1(i - p + ui, i - p + uj) += Bu[1][ui] * Bu[1][uj] * weights[j];
                    Bu2(i - p + ui, i - p + uj) += Bu[2][ui] * Bu[2][uj] * weights[j];
                }
            }
        }
    }

    MiniArray<Real> &vknots = spline->GetVKnots();
    for (int i = q; i < nv + 1; i++)
    {
        Real mid = (vknots[i + 1] + vknots[i]) / 2;
        Real ratio = (vknots[i + 1] - vknots[i]) / 2;

        // 三个积分节点分别计算值相加
        for (int j = 0; j < 3; j++)
        {
            MiniArray2D<Real> Bv = DerivBasisFuncs(2, i, mid + nodes[j] * ratio, q, vknots);
            for (int vi = 0; vi < q + 1; vi++)
            {
                for (int vj = 0; vj < q + 1; vj++)
                {
                    Bv0(i - q + vi, i - q + vj) += Bv[0][vi] * Bv[0][vj] * weights[j];
                    Bv1(i - q + vi, i - q + vj) += Bv[1][vi] * Bv[1][vj] * weights[j];
                    Bv2(i - q + vi, i - q + vj) += Bv[2][vi] * Bv[2][vj] * weights[j];
                }
            }
        }
    }

    // 累加构造系数矩阵
    int Nuv = (nu + 1) * (nv + 1);
    Eigen::MatrixXd B = (AT * A).toDense();

    // 配置权重
    Real gamma = 1 - alpha - beta;
    for (int i = 0; i < Nuv; i++)
    {
        for (int j = 0; j < Nuv; j++)
        {
            // 获得 B_{ui}'(u) * B_{vi}(v) * B_{uj}'(u) * B_{vj}(v) 对应的索引
            int ui = i / (nv + 1);
            int uj = j / (nv + 1);
            int vi = i % (nv + 1);
            int vj = j % (nv + 1);

            // 累计在 B 矩阵上
            B(i, j) += Bu1(ui, uj) * Bv0(vi, vj) * alpha / gamma;    // BuBu^T
            B(i, j) += Bu0(ui, uj) * Bv1(vi, vj) * alpha / gamma;    // BvBv^T
            B(i, j) += Bu2(ui, uj) * Bv0(vi, vj) * beta / gamma;     // BuuBuu^T
            B(i, j) += Bu0(ui, uj) * Bv2(vi, vj) * beta / gamma;     // BvvBvv^T
            B(i, j) += 2 * Bu1(ui, uj) * Bv1(vi, vj) * beta / gamma; // 2 * BuvBuv^T
        }
    }

    // Cholesky 分解
    Eigen::LLT<Eigen::MatrixXd> llt(B);

    // 构造最小二乘+能量约束系统的右端矩阵
    Eigen::MatrixXd b(Nuv, 3);
    Eigen::MatrixXd DA = AT.toDense();
    for (int i = 0; i < Nuv; i++)
    {
        b(i, 0) = b(i, 1) = b(i, 2) = 0;
        for (int j = 0; j < m + 1; j++)
        {
            b(i, 0) += DA(i, j) * m_points[j].x();
            b(i, 1) += DA(i, j) * m_points[j].y();
            b(i, 2) += DA(i, j) * m_points[j].z();
        }
    }

    // 求解线性系统
    Eigen::MatrixXd X = llt.solve(b);
    for (int i = 0; i < nu + 1; i++)
    {
        for (int j = 0; j < nv + 1; j++)
        {
            int index = i * (nv + 1) + j;
            cpts[i][j] = {X(index, 0), X(index, 1), X(index, 2)};
        }
    }

    return spline;
}

MiniBSplineSurface *MiniCloud::ChordFittingSurface(MiniArray<Real> &u, MiniArray<Real> &v, int Nu, int Nv, int p, int q, MiniArray<Real> &w, Real alpha, Real beta) const
{
    // N 个控制点
    int nu = Nu - 1;
    int nv = Nv - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineSurface *spline = new MiniBSplineSurface(nu + 1, nv + 1, p, q);
    MiniArray2D<Vector3f> &cpts = spline->GetPoints();

    clock_t start = clock();

    // 填充最小二乘系数矩阵
    Eigen::SparseMatrix<Real> A(m + 1, (nu + 1) * (nv + 1));
    Eigen::SparseMatrix<Real> AT((nu + 1) * (nv + 1), m + 1);
    for (int k = 0; k < m + 1; k++)
    {
        int uSpan = FindSpan(u[k], spline->GetUDegree(), spline->GetUKnots());
        int vSpan = FindSpan(v[k], spline->GetVDegree(), spline->GetVKnots());

        MiniArray<Real> Bu = BasisFuncs(uSpan, u[k], spline->GetUDegree(), spline->GetUKnots());
        MiniArray<Real> Bv = BasisFuncs(vSpan, v[k], spline->GetVDegree(), spline->GetVKnots());

        // 每次填充 (p + 1) * (q + 1) 规模的元素
        int uIndex = (uSpan - p) * (nv + 1);
        for (int i = 0; i < p + 1; i++)
        {
            int vIndex = vSpan - q;
            for (int j = 0; j < q + 1; j++)
            {
                // 同时设置 A 与 A^T，注意 AT 要乘上权重 w
                A.insert(k, uIndex + vIndex + j) = Bu[i] * Bv[j];
                AT.insert(uIndex + vIndex + j, k) = Bu[i] * Bv[j] * w[k];
            }
            uIndex += nv + 1;
        }
    }

    double t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
    qDebug() << "Filling LS Time: " << t;

    start = clock();

    // 构造系数矩阵，加上最小化弦长的权重，增加最小化切向变化的约束
    int Nuv = (nu + 1) * (nv + 1);
    Real gamma = 1 - alpha - beta;
    Eigen::MatrixXd B = (AT * A).toDense() * gamma;

    // 先添加对角块
    for (int i = 0; i < nu + 1; i++)
    {
        int ii = i * (nv + 1);

        // 最小化弦长的边界
        B(ii, ii) += alpha;
        B(ii, ii + 1) += -alpha;

        B(ii + nv, ii + nv) += alpha;
        B(ii + nv, ii + nv - 1) += -alpha;

        // 最小化导数的边界
        B(ii, ii) += beta;
        B(ii, ii + 1) += -2 * beta;
        B(ii, ii + 2) += beta;

        B(ii + 1, ii) += -2 * beta;
        B(ii + 1, ii + 1) += 5 * beta;
        B(ii + 1, ii + 2) += -4 * beta;
        B(ii + 1, ii + 3) += beta;

        B(ii + nv, ii + nv) += beta;
        B(ii + nv, ii + nv - 1) += -2 * beta;
        B(ii + nv, ii + nv - 2) += beta;

        B(ii + nv - 1, ii + nv) += -2 * beta;
        B(ii + nv - 1, ii + nv - 1) += 5 * beta;
        B(ii + nv - 1, ii + nv - 2) += -4 * beta;
        B(ii + nv - 1, ii + nv - 3) += beta;

        // 最小化弦长的中间部分
        for (int j = 1; j < nv; j++)
        {
            B(ii + j, ii + j) += 2 * alpha;
            B(ii + j, ii + j - 1) += -alpha;
            B(ii + j, ii + j + 1) += -alpha;
        }

        // 最小化导数的中间部分
        for (int j = 2; j < nv - 1; j++)
        {
            B(ii + j, ii + j) += 6 * beta;
            B(ii + j, ii + j - 1) += -4 * beta;
            B(ii + j, ii + j + 1) += -4 * beta;
            B(ii + j, ii + j - 2) += beta;
            B(ii + j, ii + j + 2) += beta;
        }

        // 添加最小化弦长的中间部分
        if (i > 0 && i < nu)
        {
            for (int j = 0; j < nv + 1; j++)
            {
                B(ii + j, ii + j) += 2 * alpha;
                B(ii + j, ii + j + nv + 1) += -alpha;
                B(ii + j, ii + j - (nv + 1)) += -alpha;
            }
        }

        // 添加最小化导数的中间部分
        if (i > 1 && i < nu - 1)
        {
            for (int j = 0; j < nv + 1; j++)
            {
                B(ii + j, ii + j) += 6 * beta;
                B(ii + j, ii + j + nv + 1) += -4 * beta;
                B(ii + j, ii + j - (nv + 1)) += -4 * beta;
                B(ii + j, ii + j + (nv + 1) * 2) += beta;
                B(ii + j, ii + j - (nv + 1) * 2) += beta;
            }
        }
    }

    // 处理前两行与最后两行
    for (int i = 0; i < nv + 1; i++)
    {
        // 第一行
        B(i, i) += alpha + beta;
        B(i, i + nv + 1) += -alpha - 2 * beta;
        B(i, i + (nv + 1) * 2) += beta;

        // 第二行
        B(i + nv + 1, i) += -2 * beta;
        B(i + nv + 1, i + nv + 1) += 5 * beta;
        B(i + nv + 1, i + (nv + 1) * 2) += -4 * beta;
        B(i + nv + 1, i + (nv + 1) * 3) += beta;

        // 最后一行
        int index = nu * (nv + 1) + i;
        B(index, index) += alpha + beta;
        B(index, index - (nv + 1)) += -alpha -2 * beta;
        B(index, index - (nv + 1) * 2) += beta;

        // 倒数第二行
        B(index - nv - 1, index) += -2 * beta;
        B(index - nv - 1, index - nv - 1) += 5 * beta;
        B(index - nv - 1, index - (nv + 1) * 2) += -4 * beta;
        B(index - nv - 1, index - (nv + 1) * 3) += beta;
    }

    t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
    qDebug() << "Adding Constrains Time: " << t;

    start = clock();

    // Cholesky 分解
    Eigen::LLT<Eigen::MatrixXd> llt(B);

    t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
    qDebug() << "Cholesky Time: " << t;

    start = clock();

    // 构造最小二乘+能量约束系统的右端矩阵
    Eigen::MatrixXd b(Nuv, 3);
    Eigen::MatrixXd DA = AT.toDense();
    for (int i = 0; i < Nuv; i++)
    {
        b(i, 0) = b(i, 1) = b(i, 2) = 0;
        for (int j = 0; j < m + 1; j++)
        {
            b(i, 0) += gamma * DA(i, j) * m_points[j].x();
            b(i, 1) += gamma * DA(i, j) * m_points[j].y();
            b(i, 2) += gamma * DA(i, j) * m_points[j].z();
        }
    }

    // 求解线性系统
    Eigen::MatrixXd X = llt.solve(b);
    for (int i = 0; i < nu + 1; i++)
    {
        for (int j = 0; j < nv + 1; j++)
        {
            int index = i * (nv + 1) + j;
            cpts[i][j] = {X(index, 0), X(index, 1), X(index, 2)};
        }
    }

    t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
    qDebug() << "Solving Time: " << t;

    start = clock();

    return spline;
}

MiniBSplineSurface *MiniCloud::LSPIASurface(MiniArray<Real> &u, MiniArray<Real> &v, int Nu, int Nv, int p, int q, Real tol) const
{
    // N 个控制点，m + 1 个数据点和对应参数
    int nu = Nu - 1;
    int nv = Nv - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineSurface *spline = new MiniBSplineSurface(nu + 1, nv + 1, p, q);
    MiniArray2D<Vector3f> &P = spline->GetPoints();
    const MiniArray<Vector3f> &Q = GetPoints();

    // 几何迭代法
    Real e = tol * 10;
    int times = 0;
    while (e > tol && times < ITERATION)
    {
        MiniArray2D<Real> basisSum(nu + 1, MiniArray<Real>(nv + 1));         // 记录每个控制点对应的那组基函数和
        MiniArray2D<Vector3f> deltaSum(nu + 1, MiniArray<Vector3f>(nv + 1)); // 记录每个控制点对应的那组偏差向量加权和

        // 对每个数据点计算增量
        for (int i = 0; i < m + 1; i++)
        {
            Vector3f delta = Q[i] - spline->SurfacePoint(u[i], v[i]);

            // 计算此参数对应的非零基函数值
            int ju = FindSpan(u[i], spline->GetUDegree(), spline->GetUKnots());
            int jv = FindSpan(v[i], spline->GetVDegree(), spline->GetVKnots());
            MiniArray<Real> basisFuncU = BasisFuncs(ju, u[i], spline->GetUDegree(), spline->GetUKnots());
            MiniArray<Real> basisFuncV = BasisFuncs(jv, v[i], spline->GetVDegree(), spline->GetVKnots());

            // 累计参数
            for (int ku = 0; ku < int(basisFuncU.size()); ku++)
            {
                for (int kv = 0; kv < int(basisFuncV.size()); kv++)
                {
                    basisSum[ju - p + ku][jv - q + kv] += basisFuncU[ku] * basisFuncV[kv];
                    deltaSum[ju - p + ku][jv - q + kv] = deltaSum[ju - p + ku][jv - q + kv] + delta * basisFuncU[ku] * basisFuncV[kv];
                }
            }
        }

        // 得到控制点增量的平均值并加上增量
        for (int i = 0; i < nu + 1; i++)
            for (int j = 0; j < nv + 1; j++)
                P[i][j] = P[i][j] + deltaSum[i][j] / basisSum[i][j];

        // 计算区间误差
        e = 0;
        for (int i = 0; i < m + 1; i++)
        {
            Real err = length(spline->SurfacePoint(u[i], v[i]) - m_points[i]);
            e += err * err / (m + 1);
        }
        times++;
    }
    return spline;
}

MiniBSplineSurface *MiniCloud::SDEInitialDBS(int Nu, int Nv, int p, int q, Real tol, Real ru, Real rv) const
{
    // 计算一个包围盒
    MiniArray<Vector3f> box = OrientBoundingBox(m_points);

    // N 个控制点，m + 1 个数据点和对应参数
    int nu = Nu - 1;
    int nv = Nv - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineSurface *spline = new MiniBSplineSurface(nu + 1, nv + 1, p, q);
    MiniArray2D<Vector3f> &P = spline->GetPoints();
    const MiniArray<Vector3f> &Q = GetPoints();

    // 稍微放大包围盒，不要放大原点位置
    box[0] = box[0] * ru;
    box[1] = box[1] * ru;
    box[2] = box[2] * rv;
    box[3] = box[3] * rv;

    // 根据包围盒设置初始控制点
    MiniArray<Vector3f> oxy(3);
    oxy[0] = box.back() + box[1] + box[3] + box[5];
    oxy[1] = normalize(box[0]);
    oxy[2] = normalize(box[2]);
    Real su = (length(box[0]) + length(box[1])) / nu;
    Real sv = (length(box[2]) + length(box[3])) / nv;

    for (int i = 0; i < nu + 1; i++)
        for (int j = 0; j < nv + 1; j++)
            P[i][j] = oxy[0] + oxy[1] * su * i + oxy[2] * sv * j;

    // 设置平均权重
    MiniArray<Real> w(m + 1);
    for (int i = 0; i < m + 1; i++)
        w[i] = 0.5;

    // 加细分割曲面网格
    nu *= 2, nv *= 2;

    int times = 0;
    Real e = 0, e1 = tol * 10;
    while (fabs(e - e1) > tol && times < ITERATION)
    {
        // 记录新的误差
        e = e1;

        // 构造相交网格及其参数
        MiniArray<Real> u((nu + 1) * (nv + 1));
        MiniArray<Real> v((nu + 1) * (nv + 1));
        MiniArray2D<Vector3f> R(nu + 1, MiniArray<Vector3f>(nv + 1));
        MiniArray2D<Vector3f> N(nu + 1, MiniArray<Vector3f>(nv + 1));
        for (int i = 1; i < nu; i++)
        {
            for (int j = 1; j < nv; j++)
            {
                u[i * (nv + 1) + j] = 1.0 / nu * i;
                v[i * (nv + 1) + j] = 1.0 / nv * j;

                // 内部点计算法向
                MiniArray2D<Vector3f> S = spline->SurfaceDerivPoint(1, 1.0 / nu * i, 1.0 / nv * j);
                R[i][j] = S[0][0];
                N[i][j] = normalize(cross(S[1][0], S[0][1]));
            }
        }

        // 外插计算边界点处的法向
        // 左右边界外插
        for (int i = 1; i < nu; i++)
        {
            Real t = 1.0 / nu * i;
            u[i * (nv + 1)] = t;
            v[i * (nv + 1)] = 0;
            R[i][0] = spline->SurfacePoint(t, 0);
            N[i][0] = normalize(N[i][1] * 2 - N[i][2]);

            u[i * (nv + 1) + nv] = t;
            v[i * (nv + 1) + nv] = 1;
            R[i][nv] = spline->SurfacePoint(t, 1);
            N[i][nv] = normalize(N[i][nv-1] * 2 - N[i][nv-2]);
        }

        // 上下边界外插
        for (int i = 1; i < nv; i++)
        {
            Real t = 1.0 / nv * i;
            u[i] = 0;
            v[i] = t;
            R[0][i] = spline->SurfacePoint(0, t);
            N[0][i] = normalize(N[1][i] * 2 - N[2][i]);

            u[nu * (nv + 1) + i] = 1;
            v[nu * (nv + 1) + i] = t;
            R[nu][i] = spline->SurfacePoint(1, t);
            N[nu][i] = normalize(N[nu-1][i] * 2 - N[nu-2][i]);
        }

        // 四个角点外插
        u[0] = 0, v[0] = 0;
        u[nv] = 0, v[nv] = 1;
        u[nu * (nv + 1)] = 1, v[nu * (nv + 1)] = 0;
        u[nu * (nv + 1) + nv] = 1, v[nu * (nv + 1) + nv] = 1;

        R[0][0] = spline->SurfacePoint(0, 0);
        R[0][nv] = spline->SurfacePoint(0, 1);
        R[nu][0] = spline->SurfacePoint(1, 0);
        R[nu][nv] = spline->SurfacePoint(1, 1);

        N[0][0] = normalize(N[0][1] * 2 - N[0][2]);
        N[0][nv] = normalize(N[0][nv-1] * 2 - N[0][nv-2]);
        N[nu][0] = normalize(N[nu-1][0] * 2 - N[nu-2][0]);
        N[nu][nv] = normalize(N[nu-1][nv] * 2 - N[nu-2][nv]);

        // 遍历每个控制点进行投影，计算新的误差
        e1 = 0;
        for (int i = 0; i < nu + 1; i++)
        {
            for (int j = 0; j < nv + 1; j++)
            {
                Vector c(5);

                // 对每个点云中的点计算误差
                for (int mu = 0; mu < m + 1; mu++)
                {
                    Real a = 1 / pow(length(R[i][j] - Q[mu]), 4);
                    c[0] += a;
                    c[1] += a * Q[mu].x();
                    c[2] += a * Q[mu].y();
                    c[3] += a * Q[mu].z();
                    c[4] += a * (Q[mu].x() * Q[mu].x() + Q[mu].y() * Q[mu].y() + Q[mu].z() * Q[mu].z());
                }

                // 计算出极小值参数，获得近似后的点
                Real lambda = (c[1] * N[i][j].x() + c[2] * N[i][j].y() + c[3] * N[i][j].z()) / c[0];
                Real t = lambda - dot(R[i][j], N[i][j]);

                // 计算投影点以及误差
                R[i][j] = R[i][j] + N[i][j] * t;
                e1 += length(spline->SurfacePoint(1.0 / nu * i, 1.0 / nv * j) - R[i][j]);
            }
        }
        e1 /= (nu + 1) * (nv + 1);

        MiniCloud cloud(MiniArray<Vector3f>(0));
        MiniArray<Vector3f> &final = cloud.GetPoints();
        for (int i = 0; i < nu + 1; i++)
            for (int j = 0; j < nv + 1; j++)
                final.push_back(R[i][j]);

        // 计算完成后即可移除原先曲面
        delete spline;

        // 重新拟合曲面，不需要光滑化，权重为零
        spline = cloud.ChordFittingSurface(u, v, Nu, Nv, p, q, w, 0, 0);
        times++;
    }
    return spline;
}

MiniBSplineSurface *MiniCloud::BoundaryParameterization(MiniArray<Real> &u, MiniArray<Real> &v, int Nu, int Nv, int p, int q, MiniArray<Real> &w, Real alpha, Real beta) const
{
    MiniArray<Vector3f> box = OrientBoundingBox(m_points);

    // 最长的两个方向，构造初始投影平面
    MiniArray<Vector3f> oxy(3);
    oxy[0] = box.back() + box[1] + box[3];
    oxy[1] = normalize(box[0]);
    oxy[2] = normalize(box[2]);

    // 计算投影参数
    int N = m_points.size();
    for (int i = 0; i < N; i++)
    {
        u[i] = dot(m_points[i] - oxy[0], oxy[1]);
        v[i] = dot(m_points[i] - oxy[0], oxy[2]);
    }

    // 标准化到 [0,1] 上
    u = Normalized(u);
    v = Normalized(v);

    // 计算误差
    int times = 0;
    MiniBSplineSurface *S;
    while (times < 3)
    {
        clock_t start = clock();

        // 最小化弦长拟合曲面
        S = ChordFittingSurface(u, v, Nu, Nv, p, q, w, alpha, beta);

        double t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
        qDebug() << "Fitting Time: " << t;

        start = clock();

        // 计算误差
        Real err = 0;
        for (int i = 0; i < 1000; i++)
        {
            double e = length(S->SurfacePoint(u[i], v[i]) - m_points[i]);
            err += e * e / 1000;
        }
        qDebug() << "Square Distance Error: " << err;

        t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
        qDebug() << "Error Time: " << t;

        start = clock();

        double parae = 0;
        // 计算参数误差并更新参数
        for (int i = 0; i < N; i++)
        {
            Vector uv = S->OrthoPointTorusProjection(m_points[i], {u[i], v[i]}, 1e-8);

            parae += fabs(u[i] - uv[0]) + fabs(v[i] - uv[1]);
            u[i] = uv[0];
            v[i] = uv[1];
        }

        t = double(clock() - start) / CLOCKS_PER_SEC;	// 获取时间间隔
        qDebug() << "Projection Time: " << t;

        start = clock();

        qDebug() << "Parameter Error: " << parae / N;

        qDebug() << "Nu = " << Nu << ", Nv = " << Nv;
        qDebug() << "----------------------------------";

        // 增加控制网格规模
        Nu *= 2, Nv *= 2;
        times++;

        // 清除迭代曲面
        if (times < 3)
            delete S;
    }
    return S;
}
